package com.example.beidouapplication;

import static java.lang.Math.abs;
import static java.lang.Math.floor;

public class CCoordTrans {

    static int C54 = 0;
    static int C84 = 1;
    static int CGCS2000 = 2;
//#define C54 0
//            #define C84 1
//            #define CGCS2000 2
             int BELT_UNIT=6;
 double LONGDIR84=6378137.0e0;
 double YI284=0.00669437999013e0;
 double LONGDIR54=6378245.0e0;
 double YI254=0.006693421622966e0;

 double LONGDIR_2000=6378137.0e0;			//CGCS2000					20131010
 double YI2_2000=0.0066943800229e0;		//CGCS2000					20131010

    double M_PI = Math.PI;

    double LONGDIR;
    double YI2;
    double m_dx,m_dy,m_dz,m_wx,m_wy,m_wz,m_dk;

    public CCoordTrans()
    {
        m_dx=-6.6;
        m_dy=-125.3;
        m_dz=-52.65;
        m_wx=0;
        m_wy=0;
        m_wz=0;
        m_dk=0;
        LONGDIR=LONGDIR_2000;
        YI2=YI2_2000;
    }

    void SetConvParaTo84(double cp_dx,double cp_dy,double cp_dz,
                                      double cp_wx,double cp_wy,double cp_wz,double cp_dk)
    {
        m_dx=cp_dx;
        m_dy=cp_dy;
        m_dz=cp_dz;
        m_wx=cp_wx;
        m_wy=cp_wy;
        m_wz=cp_wz;
        m_dk=cp_dk;
    }
    void DMS_RAD(int Deg,int Min,double Sec, MyPointer<Double> Rad)
    {
        if(Deg<0)
        {
            Min = -Min;
            Sec = -Sec;
        }
        Rad.setValue(Deg + Min / 60.0 + Sec / 3600.0 / 180.0 * M_PI);
        return ;
    }

    void RAD_DMS(double Rad,MyPointer<Integer> Deg, MyPointer<Integer> Min, MyPointer<Double> Sec
//                 int* Deg,int* Min,double* Sec
    )
    {
        double AR=0,AM=0;
        AR = Rad;
        if(Rad<0)	AR=-Rad;
        AR = AR+1.0e-10;
        AR = AR*180.0/M_PI;
	Deg.setValue((int) AR);
        AM = (AR-Deg.getValue())*60.0;
        Min.setValue((int) AM);
        Sec.setValue((AM - Min.getValue()) * 60.0);
        if(Rad<0) Deg.setValue(-Deg.getValue());
        return;
    }

    static double cos(double a) {
        return Math.cos(a);
    }
    static double sin(double a) {
        return Math.sin(a);
    }
    static double tan(double a) {
        return Math.tan(a);
    }
    static double atan2(double a, double b) {
        return Math.atan2(a, b);
    }
    static double sqrt(double a) {
        if (a < 0) {
            a = -a;
        }
        return Math.sqrt(a);
    }

    static double fabs(double a) {
        return abs(a);
    }

    double ConvergeAngle(double B,double l,int mode)
    {
        double YIBa2;
        double aa,aa2;
        double ConAngle;
        if (mode == C54) {
            LONGDIR=LONGDIR54;
            YI2=YI254;
        } else if (mode == C84) {
            LONGDIR = LONGDIR84;
            YI2 = YI284;
        } else if (mode == CGCS2000) {

            LONGDIR = LONGDIR_2000;
            YI2 = YI2_2000;
        } else {
            LONGDIR = LONGDIR84;
            YI2 = YI284;
        }
        YIBa2=YI2/(1-YI2);
        aa=YIBa2*cos(B)*cos(B);
        aa2=aa*aa;
        ConAngle=sin(B)*l+sin(B)*cos(B)*cos(B)*l*l*l*(1+3*aa+2*aa2)/3;
        ConAngle=ConAngle+sin(B)*cos(B)*cos(B)*cos(B)*cos(B)*l*l*l*l*l*(2-tan(B)*tan(B))/15.0;
        return ConAngle;
    }

    void XY_BLCon(double X,double Y,int num_belt,MyPointer<Double> B,MyPointer<Double> L,
                  MyPointer<Double> radCon,int mode)
    {
        int i;
        double a1,a2,a3,a4;
        double aa,bb,cc,dd;
        double C,x2,x4,x6,x8,x10;
        double K1,K2,K3;
        double[] b11 = new double[5],r11 = new double[5],d11= new double[5];
        double Nf,Bf,Tf,Y2,Vf2,Mb,Ml;
        double l;
        int DegL0;
        MyPointer<Double> L0 = new MyPointer<>(0.0);
        if (mode == C54) {
            LONGDIR=LONGDIR54;
            YI2=YI254;
        } else if (mode == C84) {
            LONGDIR = LONGDIR84;
            YI2 = YI284;
        } else if (mode == CGCS2000) {

            LONGDIR = LONGDIR_2000;
            YI2 = YI2_2000;
        } else {
            LONGDIR = LONGDIR84;
            YI2 = YI284;
        }
        C=LONGDIR/sqrt(1.0-YI2);
        x2=YI2;
        x4=x2*x2*1e0;
        x6=x2*x4*1e0;
        x8=x4*x4*1e0;
        x10=x2*x8*1e0;
        aa=1.0+3.0*x2/4.0+45.0*x4/64.0+175.0*x6/256.0;
        aa=aa+11025.0*x8/16384.0+43659.0*x10/65536.0;
        bb=3.0*x2/4.0+15.0*x4/16.0+525.0*x6/512.0;
        bb=bb+2205.0*x8/2048.0+72765.0*x10/65536.0;
        cc=15.0*x4*1e0/64.0+105.0*x6/256.0;
        cc=cc+2205.0*x8/4096.0+10395.0*x10/16384.0;
        dd=35.0*x6/512.0+315.0*x8/2048.0+31185.0*x10/131072.0;
        a1=aa*LONGDIR*(1.0-YI2);
        a2=-bb*LONGDIR*(1.0-YI2)/2.0;
        a3=cc*LONGDIR*(1.0-YI2)/4.0;
        a4=-dd*LONGDIR*(1.0-YI2)/6.0;
        b11[0]=-a2/a1;
        r11[0]=-a3/a1;
        d11[0]=-a4/a1;
        for(i=0;i<4;i++)
        {
            b11[i+1]=b11[0]+b11[0]*r11[i]-2.0*r11[0]*b11[i]-3.0*b11[0]*b11[i]*b11[i]/2.0;
            r11[i+1]=r11[0]+b11[0]*b11[i];
            d11[i+1]=d11[0]+b11[0]*r11[i]+2.0*r11[0]*b11[i]-b11[0]*b11[i]*b11[i]/2.0;
        }
        K1=2.0*b11[4]+4.0*r11[4]+6.0*d11[4];
        K2=-8.0*r11[4]-32.0*d11[4];
        K3=32.0*d11[4];
        Bf=X/a1;
        Bf=Bf+cos(Bf)*sin(Bf)*(K1+sin(Bf)*sin(Bf)*(K2+sin(Bf)*sin(Bf)*K3));
        Tf=tan(Bf);
        Y2=x2/(1.0-x2)*cos(Bf)*cos(Bf);//Nemta的平方,Nemta(F)的平方
        Vf2=1.0+Y2;
        Nf=C/sqrt(Vf2);
        Ml=Y/Nf;
        Mb=Ml*Ml;

	B.setValue( Bf-Mb*Vf2*Tf/2.0+Mb*Mb*Vf2*Tf*(5.0+3.0*Tf*Tf+Y2-9.0*Y2*Tf*Tf)/24.0
        -Mb*Mb*Mb*Vf2*Tf*(61.0+(90.0+45.0*Tf*Tf)*Tf*Tf)/720.0);

        l=Ml/cos(Bf)-Ml*Mb*(1.0+Y2+2.0*Tf*Tf)/6.0/cos(Bf)
        +Ml*Mb*Mb*(5+(6+8*Tf*Tf)*Y2+(28+24*Tf*Tf)*Tf*Tf)/120.0/cos(Bf);

        DegL0=BELT_UNIT*num_belt-3;
        DMS_RAD(DegL0,0,0,L0);
	L.setValue(l + L0.getValue());
        radCon.setValue(ConvergeAngle(B.getValue(), l, mode));

    }

    void Bl_XY(double B,double l,MyPointer<Double> X,MyPointer<Double> Y,int mode)
    {
        double a1,a2,a3,a4;
        double aa,bb,cc,dd;
        double C,x2,x4,x6,x8,x10;
        double r0,r1,r2,r3;
        double N,X0,Tb,Y2,M0,M2;

        if (mode == C54) {
            LONGDIR=LONGDIR54;
            YI2=YI254;
        } else if (mode == C84) {
            LONGDIR = LONGDIR84;
            YI2 = YI284;
        } else if (mode == CGCS2000) {

            LONGDIR = LONGDIR_2000;
            YI2 = YI2_2000;
        } else {
            LONGDIR = LONGDIR84;
            YI2 = YI284;
        }

        C=LONGDIR/sqrt(1.0-YI2);
        x2=YI2;
        x4=x2*x2*1e0;
        x6=x2*x4*1e0;
        x8=x4*x4*1e0;
        x10=x2*x8*1e0;
        aa=1.0+3.0*x2/4.0+45.0*x4/64.0+175.0*x6/256.0;
        aa=aa+11025.0*x8/16384.0+43659.0*x10/65536.0;
        bb=3.0*x2/4.0+15.0*x4/16.0+525.0*x6/512.0;
        bb=bb+2205.0*x8/2048.0+72765.0*x10/65536.0;
        cc=15.0*x4*1e0/64.0+105.0*x6/256.0;
        cc=cc+2205.0*x8/4096.0+10395.0*x10/16384.0;
        dd=35.0*x6/512.0+315.0*x8/2048.0+31185.0*x10/131072.0;
        a1=aa*LONGDIR*(1.0-YI2);
        a2=-bb*LONGDIR*(1.0-YI2)/2.0;
        a3=cc*LONGDIR*(1.0-YI2)/4.0;
        a4=-dd*LONGDIR*(1.0-YI2)/6.0;
        r0=a1;
        r1=2.0*a2+4.0*a3+6.0*a4;
        r2=-8.0*a3-32.0*a4;
        r3=32.0*a4;
        Tb=tan(B);
        Y2=x2/(1.0-x2)*cos(B)*cos(B);
        N=C/sqrt(1.0+Y2);
        X0=r0*B+cos(B)*sin(B)*(r1+sin(B)*sin(B)*(r2+sin(B)*sin(B)*r3));
        M0=cos(B)*l;
        M2=M0*M0;
	X.setValue(X0+M2*N*Tb/2.0
        +M2*M2*N*Tb/24.0*(5.0-Tb*Tb+9.0*Y2+4.0*Y2*Y2)
        +M2*M2*M2*N*Tb/720.0*(61.0+Tb*Tb*Tb*Tb*1e0-58.0*Tb*Tb));
	Y.setValue(N*M0+N*M0*M2/6.0*(1.0+Y2-Tb*Tb)
        +N*M0*M2*M2/120.0*(5.0+Tb*Tb*Tb*Tb*1e0-18.0*Tb*Tb-58.0*Tb*Tb*Y2+14.0*Y2));
        return;
    }

    void CrossBelt(MyPointer<Double> X,MyPointer<Double> Y,MyPointer<Integer> num_belt,int mode)
    {
        int i;
        double a1,a2,a3,a4;
        double aa,bb,cc,dd;
        double C,x2,x4,x6,x8,x10;
        double K1,K2,K3;
        double[] b11 = new double[5],r11 = new double[5],d11 = new double[5];
        double Nf,Bf,Tf,Y2,Vf2,Mb,Ml;
        double B;
        MyPointer<Integer> Degl = new MyPointer<>(0),Minl = new MyPointer<>(0);

        MyPointer<Double>  Secl = new MyPointer<>(0.0), l = new MyPointer<>(0.0);

        if (mode == C54) {
            LONGDIR=LONGDIR54;
            YI2=YI254;
        } else if (mode == C84) {
            LONGDIR = LONGDIR84;
            YI2 = YI284;
        } else if (mode == CGCS2000) {

            LONGDIR = LONGDIR_2000;
            YI2 = YI2_2000;
        } else {
            LONGDIR = LONGDIR84;
            YI2 = YI284;
        }

        C=LONGDIR/sqrt(1.0-YI2);
        x2=YI2;
        x4=x2*x2*1e0;
        x6=x2*x4*1e0;
        x8=x4*x4*1e0;
        x10=x2*x8*1e0;
        aa=1.0+3.0*x2/4.0+45.0*x4/64.0+175.0*x6/256.0;
        aa=aa+11025.0*x8/16384.0+43659.0*x10/65536.0;
        bb=3.0*x2/4.0+15.0*x4/16.0+525.0*x6/512.0;
        bb=bb+2205.0*x8/2048.0+72765.0*x10/65536.0;
        cc=15.0*x4*1e0/64.0+105.0*x6/256.0;
        cc=cc+2205.0*x8/4096.0+10395.0*x10/16384.0;
        dd=35.0*x6/512.0+315.0*x8/2048.0+31185.0*x10/131072.0;
        a1=aa*LONGDIR*(1.0-YI2);
        a2=-bb*LONGDIR*(1.0-YI2)/2.0;
        a3=cc*LONGDIR*(1.0-YI2)/4.0;
        a4=-dd*LONGDIR*(1.0-YI2)/6.0;
        b11[0]=-a2/a1;
        r11[0]=-a3/a1;
        d11[0]=-a4/a1;
        for(i=0;i<4;i++)
        {
            b11[i+1]=b11[0]+b11[0]*r11[i]-2.0*r11[0]*b11[i]-3.0*b11[0]*b11[i]*b11[i]/2.0;
            r11[i+1]=r11[0]+b11[0]*b11[i];
            d11[i+1]=d11[0]+b11[0]*r11[i]+2.0*r11[0]*b11[i]-b11[0]*b11[i]*b11[i]/2.0;
        }
        K1=2.0*b11[4]+4.0*r11[4]+6.0*d11[4];
        K2=-8.0*r11[4]-32.0*d11[4];
        K3=32.0*d11[4];
        Bf=X.getValue()/a1;
        Bf=Bf+cos(Bf)*sin(Bf)*(K1+sin(Bf)*sin(Bf)*(K2+sin(Bf)*sin(Bf)*K3));
        Tf=tan(Bf);
        Y2=x2/(1.0-x2)*cos(Bf)*cos(Bf);
        Vf2=1.0+Y2;
        Nf=C/sqrt(Vf2);
        Ml=Y.getValue()/Nf;
        Mb=Ml*Ml;
        B=Bf-Mb*Vf2*Tf/2.0+Mb*Mb*Vf2*Tf*(5.0+3.0*Tf*Tf+Y2-9.0*Y2*Tf*Tf)/24.0
        -Mb*Mb*Mb*Vf2*Tf*(61.0+(90.0+45.0*Tf*Tf)*Tf*Tf)/720.0;
        l.setValue(Ml/cos(Bf)-Ml*Mb*(1.0+Y2+2.0*Tf*Tf)/6.0/cos(Bf)
        +Ml*Mb*Mb*(5+(6+8*Tf*Tf)*Y2+(28+24*Tf*Tf)*Tf*Tf)/120.0/cos(Bf));
        RAD_DMS(l.getValue(),Degl,Minl,Secl);
        if(abs(Degl.getValue())==3 && (Secl.getValue()>1.0e-10||Minl.getValue()>1.0e-10))
        {
            if(Degl.getValue()<0)
            {
                num_belt.setValue(num_belt.getValue() - 1);
                Degl.setValue(2);
            }
            else if(Degl.getValue()>0)
            {
			num_belt.setValue(num_belt.getValue() + 1);
                Degl.setValue(-2);
            }
            Minl.setValue(59 - Minl.getValue());
            Secl.setValue(60 - Secl.getValue());
            DMS_RAD(Degl.getValue(),Minl.getValue(),Secl.getValue(),l);
            Bl_XY(B,l.getValue(),X,Y,mode);
        }
        return;
    }
    void WGS84Geo_BJ54Gauss(double B84,double L84,double H84,
                                         MyPointer<Double>  X54Gauss,MyPointer<Double>  Y54Gauss,MyPointer<Integer>  numBelt)
    {
        /*84坐标系所用的系列变量*/
        double N84,X84,Y84,Z84;
        /*54坐标系所用的系列变量*/
        double N54,R54,B054,X54,Y54,Z54,B54,L54,H54;
        double L054,l54;
        int tmpBelt;
        /*由B54和l54得出高斯坐标系下的值*/
        double x2,x4,x6,x8,x10,aa,bb,cc,dd;
        double a1,a2,a3,a4,r0,r1,r2,r3;
        double X0,Tb,Y2,M0,M2;
        double C;double NGauss;

        N84=LONGDIR84/sqrt(1.0-YI284*sin(B84)*sin(B84));
        X84=(N84+H84)*cos(B84)*cos(L84);
        Y84=(N84+H84)*cos(B84)*sin(L84);
        Z84=(N84*(1.0-YI284)+H84)*sin(B84);

        X54=X84+6.6;
        Y54=Y84+125.3;
        Z54=Z84+52.65;
        R54=sqrt(X54*X54+Y54*Y54);
        B054=atan2(Z54,R54);
        while(true)
        {
            N54=LONGDIR54/sqrt(1.0-YI254*sin(B054)*sin(B054));
            B54=atan2((Z54+N54*YI254*sin(B054)),R54);
            if(fabs(B54-B054)<1.0e-10) break;
            B054=B54;
        }
        L54=atan2(Y54,X54);
//	H54=R54/cos(B54-N54);

        tmpBelt= (int) ((L54*180/M_PI/6)+1);
        L054=(6*(tmpBelt )-3)*M_PI/180;
        numBelt.setValue(tmpBelt);
        l54=L54-L054;
        C=LONGDIR54/sqrt(1.0-YI254);
        x2=YI254;
        x4=x2*x2*1e0;
        x6=x2*x4*1e0;
        x8=x4*x4*1e0;
        x10=x2*x8*1e0;
        aa=1.0+3.0*x2/4.0+45.0*x4/64.0+175.0*x6/256.0;
        aa=aa+11025.0*x8/16384.0+43659.0*x10/65536.0;
        bb=3.0*x2/4.0+15.0*x4/16.0+525.0*x6/512.0;
        bb=bb+2205.0*x8/2048.0+72765.0*x10/65536.0;
        cc=15.0*x4*1e0/64.0+105.0*x6/256.0;
        cc=cc+2205.0*x8/4096.0+10395.0*x10/16384.0;
        dd=35.0*x6/512.0+315.0*x8/2048.0+31185.0*x10/131072.0;
        a1=aa*LONGDIR54*(1.0-YI254);
        a2=-bb*LONGDIR54*(1.0-YI254)/2.0;
        a3=cc*LONGDIR54*(1.0-YI254)/4.0;
        a4=-dd*LONGDIR54*(1.0-YI254)/6.0;
        r0=a1;
        r1=2.0*a2+4.0*a3+6.0*a4;
        r2=-8.0*a3-32.0*a4;
        r3=32.0*a4;
        Tb=tan(B54);
        Y2=x2/(1.0-x2)*cos(B54)*cos(B54);
        NGauss=C/sqrt(1.0+Y2);
        X0=r0*B54+cos(B54)*sin(B54)*(r1+sin(B54)*sin(B54)*(r2+sin(B54)*sin(B54)*r3));
        M0=cos(B54)*l54;
        M2=M0*M0;
	X54Gauss.setValue(X0+M2*NGauss*Tb/2.0
        +M2*M2*NGauss*Tb/24.0*(5.0-Tb*Tb+9.0*Y2+4.0*Y2*Y2)
        +M2*M2*M2*NGauss*Tb/720.0*(61.0+Tb*Tb*Tb*Tb*1e0-58.0*Tb*Tb));
	Y54Gauss.setValue(NGauss * M0 + NGauss * M0 * M2 / 6.0 * (1.0 + Y2 - Tb * Tb)
            + NGauss * M2 * M2 * M2 / 120.0 * (5.0 + Tb * Tb * Tb * Tb * 1e0 - 18.0 * Tb * Tb - 58.0 * Tb * Tb * Y2 + 14.0 * Y2));
    }
    void  WGS84_BJ54(double X84Gauss,double Y84Gauss,double H84,int iBelt,
                    MyPointer<Double> X54Gauss,MyPointer<Double> Y54Gauss,MyPointer<Double> H54)
    {
        /*84坐标系所用的系列变量*/
        double N84,X84,Y84,Z84;
        /*54坐标系所用的系列变量*/
        double N54,R54,B054,X54,Y54,Z54,B54,L54;
        MyPointer<Double> B84 = new MyPointer<>(0.0),L84 = new MyPointer<>(0.0), radcon = new MyPointer<>(0.0);
        double L054,l54;
        int tmpBelt;
        /*由B54和l54得出高斯坐标系下的值*/
        double x2,x4,x6,x8,x10,aa,bb,cc,dd;
        double a1,a2,a3,a4,r0,r1,r2,r3;
        double X0,Tb,Y2,M0,M2;
        double C;double NGauss;

        XY_BLCon(X84Gauss,Y84Gauss,iBelt,B84,L84,radcon,C84);

        N84=LONGDIR84/sqrt(1.0-YI284*sin(B84.getValue())*sin(B84.getValue()));
        X84=(N84+H84)*cos(B84.getValue())*cos(L84.getValue());
        Y84=(N84+H84)*cos(B84.getValue())*sin(L84.getValue());
        Z84=(N84*(1.0-YI284)+H84)*sin(B84.getValue());

        X54=(1-m_dk/(1.+m_dk))*X84-m_wz*Y84+m_wy*Z84-m_dx;
        Y54=(1-m_dk/(1.+m_dk))*Y84+m_wz*X84-m_wx*Z84-m_dy;
        Z54=(1-m_dk/(1.+m_dk))*Z84-m_wy*X84+m_wx*Y84-m_dz;

        R54=sqrt(X54*X54+Y54*Y54);
        B054=atan2(Z54,R54);
        while(true)
        {
            N54=LONGDIR54/sqrt(1.0-YI254*sin(B054)*sin(B054));
            B54=atan2((Z54+N54*YI254*sin(B054)),R54);
            if(fabs(B54-B054)<1.0e-10) break;
            B054=B54;
        }
        L54=atan2(Y54,X54);
//	*H54=R54/cos(B54-N54);

        tmpBelt= (int) ((L54*180/M_PI/6)+1);
        L054=(6*(tmpBelt )-3)*M_PI/180;
//	*numBelt=tmpBelt;
        l54=L54-L054;
        C=LONGDIR54/sqrt(1.0-YI254);
        x2=YI254;
        x4=x2*x2*1e0;
        x6=x2*x4*1e0;
        x8=x4*x4*1e0;
        x10=x2*x8*1e0;
        aa=1.0+3.0*x2/4.0+45.0*x4/64.0+175.0*x6/256.0;
        aa=aa+11025.0*x8/16384.0+43659.0*x10/65536.0;
        bb=3.0*x2/4.0+15.0*x4/16.0+525.0*x6/512.0;
        bb=bb+2205.0*x8/2048.0+72765.0*x10/65536.0;
        cc=15.0*x4*1e0/64.0+105.0*x6/256.0;
        cc=cc+2205.0*x8/4096.0+10395.0*x10/16384.0;
        dd=35.0*x6/512.0+315.0*x8/2048.0+31185.0*x10/131072.0;
        a1=aa*LONGDIR54*(1.0-YI254);
        a2=-bb*LONGDIR54*(1.0-YI254)/2.0;
        a3=cc*LONGDIR54*(1.0-YI254)/4.0;
        a4=-dd*LONGDIR54*(1.0-YI254)/6.0;
        r0=a1;
        r1=2.0*a2+4.0*a3+6.0*a4;
        r2=-8.0*a3-32.0*a4;
        r3=32.0*a4;
        Tb=tan(B54);
        Y2=x2/(1.0-x2)*cos(B54)*cos(B54);
        NGauss=C/sqrt(1.0+Y2);
        X0=r0*B54+cos(B54)*sin(B54)*(r1+sin(B54)*sin(B54)*(r2+sin(B54)*sin(B54)*r3));
        M0=cos(B54)*l54;
        M2=M0*M0;
	X54Gauss.setValue(X0 + M2 * NGauss * Tb / 2.0
        +M2*M2*NGauss*Tb/24.0*(5.0-Tb*Tb+9.0*Y2+4.0*Y2*Y2)
        +M2*M2*M2*NGauss*Tb/720.0*(61.0+Tb*Tb*Tb*Tb*1e0-58.0*Tb*Tb));
	Y54Gauss.setValue(NGauss*M0+NGauss*M0*M2/6.0*(1.0+Y2-Tb*Tb)
        +NGauss*M2*M2*M2/120.0*(5.0+Tb*Tb*Tb*Tb*1e0-18.0*Tb*Tb-58.0*Tb*Tb*Y2+14.0*Y2));
	H54.setValue(H84);
    }
    void BJ54_WGS84(double X54Gauss,double Y54Gauss,double H54,int iBelt,
                    MyPointer<Double> X84Gauss,MyPointer<Double> Y84Gauss,MyPointer<Double> H84)
    {
        /*84坐标系所用的系列变量*/
        double N84,X84,Y84,Z84;
        /*54坐标系所用的系列变量*/
        double N54,R84,B084,X54,Y54,Z54,B84,L84;
        double L084,l84;
        MyPointer<Double>B54= new MyPointer<>(0.0),L54 = new MyPointer<>(0.0) , radcon = new MyPointer<>(0.0);
        int tmpBelt;
        /*由B54和l54得出高斯坐标系下的值*/
        double x2,x4,x6,x8,x10,aa,bb,cc,dd;
        double a1,a2,a3,a4,r0,r1,r2,r3;
        double X0,Tb,Y2,M0,M2;
        double C;double NGauss;

        XY_BLCon(X54Gauss,Y54Gauss,iBelt,B54,L54,radcon,C54);

        N54=LONGDIR54/sqrt(1.0-YI254*sin(B54.getValue())*sin(B54.getValue()));
        X54=(N54+H54)*cos(B54.getValue())*cos(L54.getValue());
        Y54=(N54+H54)*cos(B54.getValue())*sin(L54.getValue());
        Z54=(N54*(1.0-YI254)+H54)*sin(B54.getValue());

        X84=(1+m_dk)*X54+m_wz*Y54-m_wy*Z54+m_dx;
        Y84=(1+m_dk)*Y54-m_wz*X54+m_wx*Z54+m_dy;
        Z84=(1+m_dk)*Z54+m_wy*X54-m_wx*Y54+m_dz;

        R84=sqrt(X84*X84+Y84*Y84);
        B084=atan2(Z84,R84);
        while(true)
        {
            N84=LONGDIR84/sqrt(1.0-YI284*sin(B084)*sin(B084));
            B84=atan2((Z84+N84*YI284*sin(B084)),R84);
            if(fabs(B84-B084)<1.0e-10) break;
            B084=B84;
        }
        L84=atan2(Y84,X84);
//	*H84=R84/cos(B84-N84);

        tmpBelt= (int) ((L84*180/M_PI/6)+1);
        L084=(6*(tmpBelt )-3)*M_PI/180;
//	*numBelt=tmpBelt;
        l84=L84-L084;
        C=LONGDIR84/sqrt(1.0-YI284);
        x2=YI284;
        x4=x2*x2*1e0;
        x6=x2*x4*1e0;
        x8=x4*x4*1e0;
        x10=x2*x8*1e0;
        aa=1.0+3.0*x2/4.0+45.0*x4/64.0+175.0*x6/256.0;
        aa=aa+11025.0*x8/16384.0+43659.0*x10/65536.0;
        bb=3.0*x2/4.0+15.0*x4/16.0+525.0*x6/512.0;
        bb=bb+2205.0*x8/2048.0+72765.0*x10/65536.0;
        cc=15.0*x4*1e0/64.0+105.0*x6/256.0;
        cc=cc+2205.0*x8/4096.0+10395.0*x10/16384.0;
        dd=35.0*x6/512.0+315.0*x8/2048.0+31185.0*x10/131072.0;
        a1=aa*LONGDIR84*(1.0-YI284);
        a2=-bb*LONGDIR84*(1.0-YI284)/2.0;
        a3=cc*LONGDIR84*(1.0-YI284)/4.0;
        a4=-dd*LONGDIR84*(1.0-YI284)/6.0;
        r0=a1;
        r1=2.0*a2+4.0*a3+6.0*a4;
        r2=-8.0*a3-32.0*a4;
        r3=32.0*a4;
        Tb=tan(B84);
        Y2=x2/(1.0-x2)*cos(B84)*cos(B84);
        NGauss=C/sqrt(1.0+Y2);
        X0=r0*B84+cos(B84)*sin(B84)*(r1+sin(B84)*sin(B84)*(r2+sin(B84)*sin(B84)*r3));
        M0=cos(B84)*l84;
        M2=M0*M0;
        X84Gauss.setValue(X0 + M2 * NGauss * Tb / 2.0
                + M2 * M2 * NGauss * Tb / 24.0 * (5.0 - Tb * Tb + 9.0 * Y2 + 4.0 * Y2 * Y2)
                + M2 * M2 * M2 * NGauss * Tb / 720.0 * (61.0 + Tb * Tb * Tb * Tb * 1e0 - 58.0 * Tb * Tb));
        Y84Gauss.setValue(NGauss * M0 + NGauss * M0 * M2 / 6.0 * (1.0 + Y2 - Tb * Tb)
                + NGauss * M2 * M2 * M2 / 120.0 * (5.0 + Tb * Tb * Tb * Tb * 1e0 - 18.0 * Tb * Tb - 58.0 * Tb * Tb * Y2 + 14.0 * Y2));
        H84.setValue(H54);
    }
    void BJ54Gauss_WGS84Geo(double X54Gauss, double Y54Gauss, double H54, MyPointer<Double> B84, MyPointer<Double> L84, MyPointer<Double> H84)
    {
        /*84坐标系所用的系列变量*/
        double N84,X84,Y84,Z84;
        /*54坐标系所用的系列变量*/
        double N54,R84,B084,X54,Y54,Z54,B54,L54,tB84,tL84;

        int i;
        double a1,a2,a3,a4;
        double aa,bb,cc,dd;
        double C,x2,x4,x6,x8,x10;
        double K1,K2,K3;
        double[] b11 = new double[5], r11= new double[5],d11= new double[5];
        double Nf,Bf,Tf,Y2,Vf2,Mb,Ml;
        double l=0;

        int BELT_UNIT = 6;
        int num_belt=(int)floor(Y54Gauss/1e6);
        double x = X54Gauss;
        double y = Y54Gauss-5e5-num_belt*1e6;

        double LONGDIR54=6378245.0e0;
        double YI254=0.006693421622966e0;
        double LONGDIR84=6378137.0e0;
        double YI284=0.00669437999013e0;

        double dx = -6.6;
        double dy = -125.3;
        double dz = -52.65;
        double wx = 0;
        double wy = 0;
        double wz = 0;
        double dk = 0;

        C=LONGDIR54/sqrt(1.0-YI254);
        x2=YI254;
        x4=x2*x2*1e0;
        x6=x2*x4*1e0;
        x8=x4*x4*1e0;
        x10=x2*x8*1e0;
        aa=1.0+3.0*x2/4.0+45.0*x4/64.0+175.0*x6/256.0;
        aa=aa+11025.0*x8/16384.0+43659.0*x10/65536.0;
        bb=3.0*x2/4.0+15.0*x4/16.0+525.0*x6/512.0;
        bb=bb+2205.0*x8/2048.0+72765.0*x10/65536.0;
        cc=15.0*x4*1e0/64.0+105.0*x6/256.0;
        cc=cc+2205.0*x8/4096.0+10395.0*x10/16384.0;
        dd=35.0*x6/512.0+315.0*x8/2048.0+31185.0*x10/131072.0;
        a1=aa*LONGDIR54*(1.0-YI254);
        a2=-bb*LONGDIR54*(1.0-YI254)/2.0;
        a3=cc*LONGDIR54*(1.0-YI254)/4.0;
        a4=-dd*LONGDIR54*(1.0-YI254)/6.0;
        b11[0]=-a2/a1;
        r11[0]=-a3/a1;
        d11[0]=-a4/a1;
        for(i=0;i<4;i++)
        {
            b11[i+1]=b11[0]+b11[0]*r11[i]-2.0*r11[0]*b11[i]-3.0*b11[0]*b11[i]*b11[i]/2.0;
            r11[i+1]=r11[0]+b11[0]*b11[i];
            d11[i+1]=d11[0]+b11[0]*r11[i]+2.0*r11[0]*b11[i]-b11[0]*b11[i]*b11[i]/2.0;
        }
        K1=2.0*b11[4]+4.0*r11[4]+6.0*d11[4];
        K2=-8.0*r11[4]-32.0*d11[4];
        K3=32.0*d11[4];
        Bf=x/a1;
        Bf=Bf+cos(Bf)*sin(Bf)*(K1+sin(Bf)*sin(Bf)*(K2+sin(Bf)*sin(Bf)*K3));
        Tf=tan(Bf);
        Y2=x2/(1.0-x2)*cos(Bf)*cos(Bf);//Nemta的平方,Nemta(F)的平方
        Vf2=1.0+Y2;
        Nf=C/sqrt(Vf2);
        Ml=y/Nf;
        Mb=Ml*Ml;
        B54=Bf-Mb*Vf2*Tf/2.0+Mb*Mb*Vf2*Tf*(5.0+3.0*Tf*Tf+Y2-9.0*Y2*Tf*Tf)/24.0
        -Mb*Mb*Mb*Vf2*Tf*(61.0+(90.0+45.0*Tf*Tf)*Tf*Tf)/720.0;
        l=Ml/cos(Bf)-Ml*Mb*(1.0+Y2+2.0*Tf*Tf)/6.0/cos(Bf)
        +Ml*Mb*Mb*(5+(6+8*Tf*Tf)*Y2+(28+24*Tf*Tf)*Tf*Tf)/120.0/cos(Bf);
        L54=(BELT_UNIT*num_belt-3)*M_PI/180+l;

        N54=LONGDIR54/sqrt(1.0-YI254*sin(B54)*sin(B54));
        X54=(N54+H54)*cos(B54)*cos(L54);
        Y54=(N54+H54)*cos(B54)*sin(L54);
        Z54=(N54*(1.0-YI254)+H54)*sin(B54);

        X84=(1+dk)*X54+wz*Y54-wy*Z54+dx;
        Y84=(1+dk)*Y54-wz*X54+wx*Z54+dy;
        Z84=(1+dk)*Z54+wy*X54-wx*Y54+dz;

        R84=sqrt(X84*X84+Y84*Y84);
        B084=atan2(Z84,R84);
        while(true)
        {
            N84=LONGDIR84/sqrt(1.0-YI284*sin(B084)*sin(B084));
            tB84=atan2((Z84+N84*YI284*sin(B084)),R84);
            if(fabs(tB84-B084)<1.0e-10) break;
            B084=tB84;
        }
        tL84=atan2(Y84,X84);

        B84.setValue(tB84 * 180 / M_PI);
	L84.setValue(tL84 * 180 / M_PI);
	H84.setValue(H54);
    }
//    int matherr (struct exception *a)
//    {
//        if (a->type == DOMAIN)
//            if (!strcmp(a->name,"sqrt")) {
//                a->retval = sqrt (-(a->arg1));
//                return 1;
//            }
//        return 0;
//    }
}
